#include "Models/Prob_riemann.hpp"
#include "KernelHMC.hpp"
using namespace arma; 

template<class Proposal>
class KernelHMC<Prob_riemann,Proposal>
{
	public:
		KernelHMC(Prob_riemann *d, Proposal *p, mat S)
		{
			_d=d;
			//_p=malloc(sizeof(Proposal));
			_p=p;
			boost::random::uniform_01<> U;
			runif=new RandomG::Random<boost::random::uniform_01<> >(U);
			_Sig=1;
			_step=10;
			vec eigval;
			mat eigvec;
			eps=0.00000000000001;//_step/(4*sqrt(min(S.diag())));
			//eig_sym(eigval, eigvec, S);
			//eps=1;//sqrt(as_scalar(max(eigval)));
			cout << "eps " << eps;
			_M=inv(S);

						
		}
		~KernelHMC(){
			delete runif;
		}
		KernelHMC<Prob_riemann,Proposal>(const KernelHMC<Prob_riemann,Proposal>& X ){
		}
		
		void Move(mat *X,int _n)//Move matrix convert to multidimentional theta 
		{		//, moves each column dimentionis in line
			int n=X->n_rows;//Y and X as pointers
			int m=X->n_cols;
			if(_bool==1)
			{
				Moved.zeros();
			}
			mat Y(n,m);
			Y.fill(0);
			mat xt;
			_p->Set_n(m);
			_sum=0;
			_i=0;
			for(int i=0;i<n;i++)
			{
				Y(i,span::all)=Prop((*X)(i,span::all));//Y[i]=p->r(X[i])
				mat xt=Y.row(i);
				xt=Accept(X->row(i),Y.row(i),_n);
				(*X)(i,span::all)=xt;			
				_i++;
			}
			_accept=(double)_sum/n;

			cout << "accept = " << (double)_sum/n;

			//////////
			//delete[] y;
		}	
		mat Prop(mat X)
		{
			mat Y;
			mat Yt;
			mat Y1;
			mat Xt;
			_Y1=(_p->r(1)).t();
			for(int j=0;j<_step;j++)
			{
			//	cout << X;
				if(j==0){Y1=_Y1;}
				Y=Y1+0.5*eps*(DH(X,Y1)).t();
				//cout << "Y " << Y;
				//cout << (_d->Grad(X.t()));
				try{
					Xt=X+eps*Y*inv(_d->Gt(X.t()));
				}catch(std::exception e)
				{
					cout << _d->Gt(X.t());
				}
				Yt=Y+0.5*eps*(DH(Xt,Y)).t();
				X=Xt;
				Y1=Yt;
				_Yt=Yt;
				//cout << j;
			}
				//cout << Yt;
			return Xt;

		}


		mat DH(mat X, mat Y1)
		{
			int p=X.n_elem;
			mat DG=(_d->Grad(X.t())).t();
			mat Y(p,1);

			for(int j=0;j<p;j++)
			{
				mat ig;
				mat dg=_d->dG(X.t(),j);
				try{
				ig=inv(_d->Gt(X.t()));
				}catch(std::exception e)
				{
					cout << _d->Gt(X.t());
					cout << X.t();
				}
				Y(j,0)=as_scalar(DG(0,j)-0.5*trace(ig*dg)+0.5*Y1*ig*dg*ig*Y1.t());
			}
			return Y; 
		}

		mat Accept(mat X, mat Y,int n)
		{
					mat res=X;
					int d=X.n_elem;
					//cout << "Y1 " <<_Y1;
					if(_bool==1)
					{
						double rho=0;
						double temp=_d->Density_utn(Y.t(),n)-(0.5)*log(pow(2*PI,d)*det(_d->Gt(Y.t())))-0.5*as_scalar(_Yt*inv(_d->Gt(Y.t()))*_Yt.t());//-H(thetastar)
						rho=(temp)-(_d->Density_utn(X.t(),n)-(0.5)*log(pow(2*PI,d)*det(_d->Gt(X.t())))-0.5*as_scalar(_Y1*inv(_d->Gt(X.t()))*_Y1.t()));//-H(thetastar)--H(theta)
						double u=log((*runif)());
						//cout << exp(rho) << "\n";
						if(u<rho)
						{
							Dens(_i,0)=temp;
							res=Y;
							_sum+=1;
							Moved(_i,0)=1;
						}else{
							Dens(_i,0)=temp;
						}
					}else{
						
						double rho=0;
						double temp=_d->Density_utn(Y.t(),n)-(0.5)*log(pow(2*PI,d)*det(_d->Gt(Y.t())))-0.5*as_scalar(_Yt*inv(_d->Gt(Y.t()))*_Yt.t());
						rho=temp-(Dens(_i,0));
						double u=log((*runif)());
						if(u<rho)
						{
							res=Y;
							_sum+=1;
							Dens(_i,0)=temp;
							Moved(_i,0)=1;
						}

					}
					return res;

		}
		void Set_s(mat X){
					eps*=_Sig;
		}
		//void Set_nstep(int fact){_nstep=(int)(_nstep*fact);}
		double Get_accept(void){return _accept;	}
	 	void Set_Sigma(double x){_Sig=_Sig*x;}
		void Set_bool(int b){_bool=b;}
		void Set_n(int b){
			_n=b;
			mat De(_n,1);
			mat M(_n,1);
			Moved=M;
			Dens=De;
		}
		double nMove(void)
		{
			return as_scalar(mean(Moved,0));
		}
	private:
		int _sum;
		Prob_riemann *_d;
		Proposal *_p;
		int _bool;
		RandomG::Random<boost::random::uniform_01<> > *runif;
		mat Dens; 
		int _n;
		double _accept;
		double _Sig;
		int _i;
		double eps;
		int _step;
		mat Moved;
		mat _M;//mass matrix
		mat _Y1;
		mat _Yt;

};
